
/*
   DRV8833 for  MiniCar with Arduino Pro Mini
     * https://github.com/TheArduinist/DRV8833
*/
#ifndef DRV8833_H
#define DRV8833_H

// 3, 5, 6, 9, 10, or 11
#define LEFT_FORWARD 3 // AIN1 绿色
#define LEFT_BACK 5    // AIN2 黄色

#define RIGHT_FORWARD 6 //  BIN1 绿色
#define RIGHT_BACK 9    // BIN2 黄色

// car action code
const int CAR_FORWARD = 1;
const int CAR_BACKWARD = 2;
const int CAR_STEP_LEFT = 3;
const int CAR_STEP_RIGHT = 4;
const int CAR_ON_LEFT = 30;
const int CAR_ON_RIGHT = 40;
const int CAR_CIRCLE_LEFT = 31;
const int CAR_CIRCLE_RIGHT = 41;
const int CAR_STOP = 5;
// speed
const int CAR_SPEED_UP = 6;
const int CAR_SPEED_DOWN = 7;
const int CAR_LEFT_SPEED_UP = 8;
const int CAR_LEFT_SPEED_DOWN = 9;
const int CAR_RIGHT_SPEED_UP = 8;
const int CAR_RIGHT_SPEED_DOWN = 9;

// car running state
int car_state = CAR_STOP;
int cur_car_cmd = CAR_STOP;

// PWM - speed
int left_speed = 200;
int right_speed = 200;
const int speed_step = 10;

void car_forward()
{
  if (left_speed>right_speed)
  {
     right_speed=left_speed;
  }
  else{
     left_speed=right_speed;
  }
  analogWrite(LEFT_FORWARD, left_speed);
  digitalWrite(LEFT_BACK, LOW);
  analogWrite(RIGHT_FORWARD, right_speed);
  digitalWrite(RIGHT_BACK, LOW);
}

void car_back()
{
  if (left_speed>right_speed)
  {
     right_speed=left_speed;

  }
  else{
     left_speed=right_speed;
  }
  digitalWrite(LEFT_FORWARD, LOW);
  analogWrite(LEFT_BACK, left_speed);
  digitalWrite(RIGHT_FORWARD, LOW);
  analogWrite(RIGHT_BACK, right_speed);
}

void car_stop()
{
  digitalWrite(LEFT_FORWARD, LOW);
  digitalWrite(LEFT_BACK, LOW);
  digitalWrite(RIGHT_FORWARD, LOW);
  digitalWrite(RIGHT_BACK, LOW);
}

// zero turn
void car_turn_left()
{
  left_speed=0;
  analogWrite(LEFT_FORWARD,  left_speed);
  digitalWrite(LEFT_BACK, LOW);
  analogWrite(RIGHT_FORWARD, right_speed);
  digitalWrite(RIGHT_BACK, LOW);
}


void car_turn_step_left()
{
  car_turn_left();
  car_state = CAR_STEP_LEFT;
  delay(100);
  car_stop();
}

// zero turn
void car_turn_right()
{
  analogWrite(LEFT_FORWARD, left_speed);
  digitalWrite(LEFT_BACK, LOW);
  right_speed=0;
  analogWrite(RIGHT_FORWARD, right_speed);
  digitalWrite(RIGHT_BACK, LOW);
}

void car_turn_step_right()
{
  car_turn_right();
  car_state = CAR_STEP_RIGHT;
  delay(100);
  car_stop();
}

void car_circle_left()
{
  digitalWrite(LEFT_FORWARD, LOW);
  analogWrite(LEFT_BACK, left_speed);
  analogWrite(RIGHT_FORWARD, right_speed);
  digitalWrite(RIGHT_BACK, LOW);
}

void car_circle_right()
{
  analogWrite(LEFT_FORWARD, left_speed);
  digitalWrite(LEFT_BACK, LOW);
  digitalWrite(RIGHT_FORWARD, LOW);
  analogWrite(RIGHT_BACK, right_speed);
}

void running_with_new_speed()
{
  switch (car_state)
  {
  case CAR_FORWARD:
    car_forward();
    break;
  case CAR_BACKWARD:
    car_back();
    break;
  case CAR_STEP_LEFT:
    car_turn_step_left();
    break;
  case CAR_STEP_RIGHT:
    car_turn_step_right();
    break;
  default:
    break;
  }
}

void car_left_speed_up()
{
  left_speed += speed_step;
  if (left_speed > 255)
  {
    left_speed = 255;
  }
  running_with_new_speed();
}

void car_left_speed_up()
{
  right_speed += speed_step;
  if (right_speed > 255)
  {
    right_speed = 255;
  }
  running_with_new_speed();
}

void car_speed_up()
{
  right_speed += speed_step;
  if (right_speed > 255)
  {
    right_speed = 255;
  }

  left_speed += speed_step;
  if (left_speed > 255)
  {
    left_speed = 255;
  }
  running_with_new_speed();
}

void car_left_speed_down()
{
  left_speed -= speed_step;
  if (left_speed < 0)
  {
    left_speed = 0;
  }
  running_with_new_speed();
}

void car_left_speed_down()
{
  right_speed -= speed_step;
  if (right_speed < 0)
  {
    right_speed = 0;
  }
  running_with_new_speed();
}

void car_speed_down()
{
  right_speed -= speed_step;
  if (right_speed < 0)
  {
    right_speed = 0;
  }

  left_speed -= speed_step;
  if (left_speed < 0)
  {
    left_speed = 0;
  }
  running_with_new_speed();
}

void car_action(int car_cmd)
{
  switch (car_cmd)
  {
  case CAR_FORWARD:
    car_forward();
    car_state = CAR_FORWARD;
    break;
  case CAR_ON_LEFT:
    car_turn_left();
    car_state = CAR_ON_LEFT;
    break;
  case CAR_ON_RIGHT:
    car_turn_right();
    car_state = CAR_ON_RIGHT;
    break;
  case CAR_STEP_LEFT:
    car_turn_step_left();
    car_state = CAR_STOP;
    break;
  case CAR_STEP_RIGHT:
    car_turn_step_right();
    car_state = CAR_STOP;
    break;
  case CAR_BACKWARD:
    car_back();
    car_state = CAR_BACKWARD;
    break;
  case CAR_STOP:
    car_stop();
    car_state = CAR_STOP;
    break;
  case CAR_SPEED_UP:
    car_speed_up();
    break;
  case CAR_SPEED_DOWN:
    car_speed_down();
    break;
  case CAR_LEFT_SPEED_UP:
    car_left_speed_up();
    break;
  case CAR_LEFT_SPEED_DOWN:
    car_left_speed_down();
    break;
  case CAR_RIGHT_SPEED_UP:
    car_right_speed_up();
    break;
  case CAR_RIGHT_SPEED_DOWN:
    car_right_speed_down();
    break;
  default:
    break;
  } // switch
};

void setup_drv8833()
{
  pinMode(LEFT_FORWARD, OUTPUT);
  pinMode(LEFT_BACK, OUTPUT);
  pinMode(RIGHT_FORWARD, OUTPUT);
  pinMode(RIGHT_BACK, OUTPUT);
  car_stop();
}

void loop_drv8833()
{
}

#endif /* DRV8833_H */